/**
 *
 */
package jp.seraph.jsade.model;

import jp.seraph.jsade.core.Player;
import jp.seraph.jsade.math.Angle;
import jp.seraph.jsade.math.AngleVelocity;
import jp.seraph.jsade.math.PolynomialFunction;

/**
 * ひとつの多項式（関数）を用いて角速度の計算を行う計算機
 *
 * 計算の詳細に関してはcalcメソッドのコメントを参照すること。
 */
public class PolynomialAngleVelocityCalculator implements AngleVelocityCalculator {
    /**
     * 奇数番目引数を係数部 偶数番目引数を指数部として、このインスタンスを生成するコンストラクタ ScaleFactorは1.0固定
     *
     * @param aParameters
     */
    public PolynomialAngleVelocityCalculator(double... aParameters) {
        this.init(1.0, new PolynomialFunction(aParameters));
    }

    /**
     * ScaleFactorを1.0として、このインスタンスを生成するコンストラクタ
     *
     * @param aFunction
     */
    public PolynomialAngleVelocityCalculator(PolynomialFunction aFunction) {
        this(1.0, aFunction);
    }

    /**
     *
     * @param aScaleFactor
     *            Functionに引数を渡す際に、引数にかかる倍率
     * @param aFunction
     *            計算に使用する関数
     */
    public PolynomialAngleVelocityCalculator(double aScaleFactor, PolynomialFunction aFunction) {
        this.init(aScaleFactor, aFunction);
    }

    private void init(double aScaleFactor, PolynomialFunction aEntries) {
        mScaleFactor = aScaleFactor;
        mFunction = aEntries;
    }

    private PolynomialFunction mFunction;

    private double mScaleFactor;

    public PolynomialFunction getFunction() {
        return mFunction;
    }

    public void setEntry(PolynomialFunction aEntry) {
        mFunction = aEntry;
    }

    public double getScaleFactor() {
        return mScaleFactor;
    }

    public void setScaleFactor(double aScaleFactor) {
        mScaleFactor = aScaleFactor;
    }

    /**
     * このインスタンスが保持する関数によって、角速度を計算する。 このコメント内での角度の単位は、特に示されていない場合、ラジアンである。
     *
     * 角度差 = 目標角度 - 現在角度 とする。 ABS(値)とは値の絶対値を表す。
     *
     * ABS(角度差)がPI/180未満である場合（1°未満である場合）このメソッドは常に0の角速度を返す。
     *
     * 関数には、角度差が引数として与えられる
     *
     * このインスタンスが保持する関数の結果をResultとする。 このとき、このメソッドは以下の値を返す。 if(角度差 < 0)
     * AngleVelocity.createAsRad(-Result) else AngleVelocity.createAsRad(Result)
     *
     * @see jp.seraph.jsade.model.AngleVelocityCalculator#calc(jp.seraph.jsade.core.Player,
     *      jp.seraph.jsade.model.JointIdentifier, jp.seraph.jsade.math.Angle)
     */

    public AngleVelocity calc(Player aPlayer, JointIdentifier aTargetJoint, Angle aTargetAngle) {
        Angle tCurrentAngle = aPlayer.getJointAngle(aTargetJoint);
        double tDegreeSub = aTargetAngle.getDegree() - tCurrentAngle.getDegree();

        if (Math.abs(tDegreeSub) < 1.0) {
            return AngleVelocity.ZERO;
        }

        int tMinus = tDegreeSub < 0 ? -1 : 1;
        double tResult = 0.0;
        double tCurrent = tCurrentAngle.getRad();
        double tTarget = aTargetAngle.getRad();
        double tDiff = Math.abs(tTarget - tCurrent);
        double tArg = tDiff;

        tResult = mFunction.eval(tArg * mScaleFactor);

        return AngleVelocity.createAsDegree(tResult * tMinus);
    }
}
